/*
 * Copyright (C) 2006-2010 - Frictional Games
 *
 * This file is part of HPL1 Engine.
 *
 * HPL1 Engine is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * HPL1 Engine is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with HPL1 Engine.  If not, see <http://www.gnu.org/licenses/>.
 */
#include "physics/CharacterBody.h"

#include "physics/CollideShape.h"
#include "physics/PhysicsWorld.h"
#include "physics/PhysicsBody.h"
#include "system/LowLevelSystem.h"
#include "scene/Camera3D.h"
#include "math/Math.h"

#include "game/Game.h"
#include "scene/Scene.h"
#include "scene/World3D.h"
#include "scene/PortalContainer.h"

namespace hpl {

	//////////////////////////////////////////////////////////////////////////
	// CONSTRUCTORS
	//////////////////////////////////////////////////////////////////////////

	//-----------------------------------------------------------------------

	iCharacterBody::iCharacterBody(const tString &asName, iPhysicsWorld *apWorld, const cVector3f avSize)
	{
		msName = asName;

		mpWorld = apWorld;

		mvSize = avSize;

		mbActive = true;

		mbCollideCharacter = true;

		mbTestCollision = true;

		float fRadius = cMath::Max(avSize.x, avSize.z)*0.5f;
		cMatrixf mtxOffset = cMath::MatrixRotateZ(kPi2f);
		iCollideShape *pCollider = NULL;

		if(fabs(fRadius*2.0f - avSize.y)<0.01)
			pCollider = mpWorld->CreateSphereShape(fRadius,NULL);
		else
			pCollider = mpWorld->CreateCylinderShape(fRadius, avSize.y,&mtxOffset);
			//pCollider = mpWorld->CreateCapsuleShape(fRadius, avSize.y,&mtxOffset);

		mpBody = mpWorld->CreateBody(asName,pCollider);
		mpBody->SetMass(0);
		mpBody->SetGravity(false);
		mpBody->SetIsCharacter(true);
		mpBody->SetCharacterBody(this);

		mvExtraBodies.push_back(mpBody);

		mfYaw =0;
		mfPitch =0;

		//Set move properties
		for(int i=0; i<eCharDir_LastEnum; i++)
		{
			mfMaxPosMoveSpeed[i]=10;
			mfMaxNegMoveSpeed[i]=-10;
			mfMoveSpeed[i]=0;
			mfMoveAcc[i]=20;
			mfMoveDeacc[i]=20;
			mbMoving[i]=false;
		}

		mvForce = cVector3f(0,0,0);
		mvVelolcity = cVector3f(0,0,0);

		mbGravityActive = true;

		mfMaxGravitySpeed = 30.0f;

		mbEnableNearbyBodies = false;

		mpCamera = NULL;
		mvCameraPosAdd = cVector3f(0,0,0);

		mpEntity = NULL;
		m_mtxEntityOffset = cMatrixf::Identity;
		m_mtxEntityPostOffset = cMatrixf::Identity;

		mpUserData = NULL;

		mlCameraSmoothPosNum =0;
		mlEntitySmoothPosNum =0;

		mfMaxStepHeight = mvSize.y *0.2f;
		mfStepClimbSpeed = 1.0f;
		mfClimbForwardMul = 1.0f;
		mfClimbHeightAdd = 0.01f;
		mbClimbing = false;
		mbAccurateClimbing = false;

		mfCheckStepClimbCount = 0;
		mfCheckStepClimbInterval = 1/20.0f;


		mfGroundFriction =0.1f;
		mfAirFriction = 0.01f;

		mpRayCallback = hplNew( cCharacterBodyRay, () );
		mpCollideCallbackGravity = hplNew( cCharacterBodyCollideGravity,());
		mpCollideCallbackPush = hplNew( cCharacterBodyCollidePush,());

		mpCollideCallbackGravity->mpCharBody = this;
		mpCollideCallbackPush->mpCharBody = this;

		mfMass = 1;

		mfMaxPushMass = 0;
		mfPushForce = 0;
		mbPushIn2D = true;

		mpCallback = NULL;

		mpAttachedBody = NULL;
		mbAttachmentJustAdded = true;


		mbCustomGravity = false;
		mvCustomGravity = cVector3f(0,9.8f,0);
	}

	//-----------------------------------------------------------------------

	iCharacterBody::~iCharacterBody()
	{
		for(size_t i=0; i< mvExtraBodies.size(); i++)
			mpWorld->DestroyBody(mvExtraBodies[i]);

		hplDelete(mpRayCallback);
		hplDelete(mpCollideCallbackGravity);
		hplDelete(mpCollideCallbackPush);
	}

	//-----------------------------------------------------------------------

	//////////////////////////////////////////////////////////////////////////
	// RAY CALLBACK
	//////////////////////////////////////////////////////////////////////////

	//-----------------------------------------------------------------------

	cCharacterBodyRay::cCharacterBodyRay()
	{

	}

	//-----------------------------------------------------------------------

	void cCharacterBodyRay::Clear()
	{
		mfMinDist = 10000.0f;
		mbCollide = false;
	}

	//-----------------------------------------------------------------------

	bool cCharacterBodyRay::OnIntersect(iPhysicsBody *pBody,cPhysicsRayParams *apParams)
	{
		if(	pBody->IsCharacter()==false && pBody->GetCollideCharacter() &&
			apParams->mfDist < mfMinDist)
		{
			mfMinDist = apParams->mfDist;
			mbCollide = true;
		}

		return true;
	}

	//-----------------------------------------------------------------------

	//////////////////////////////////////////////////////////////////////////
	// COLLIDE GRAVITY CALLBACK
	//////////////////////////////////////////////////////////////////////////

	//-----------------------------------------------------------------------

	cCharacterBodyCollideGravity::cCharacterBodyCollideGravity()
	{
		mpCharBody = NULL;
	}

	//-----------------------------------------------------------------------

	void cCharacterBodyCollideGravity::OnCollision(iPhysicsBody *apBody, cCollideData *apCollideData)
	{
		if(apBody->GetCanAttachCharacter())
		{
			if(mpCharBody->GetAttachedBody()==NULL)
			{
				mpCharBody->SetAttachedBody(apBody);
			}
		}

		if(apBody->GetMass()==0 || apBody->GetPushedByCharacterGravity()==false) return;

		bool bPushDown=false;
		cVector3f vPoint(0,0,0);
		float fNumPoints=0;


		//Go through all of the contact points and check if any is a movement up.
		//This means the body is below the character and should be pushed down.
		for(int i=0; i< apCollideData->mlNumOfPoints; i++)
		{
			//TODO: Get the point at which to apply the force.
			cCollidePoint &point = apCollideData->mvContactPoints[i];
			if(point.mvNormal.y > 0.001f)
			{
				bPushDown = true;
				fNumPoints+=1;
				vPoint += point.mvPoint;
			}
		}

		if(bPushDown)
		{
			float fForceAdd =0;

			//Totally unrealistic force add
			//Skip this for now and add better with more object specific stuff
			/*if(mpCharBody->GetForceVelocity().y < 0)
			{
				fForceAdd = mpCharBody->GetForceVelocity().y * mpCharBody->GetMass() * 10;
			}*/

			vPoint = vPoint / fNumPoints;
			apBody->AddForceAtPosition(cVector3f(0,mpCharBody->GetMass() * -9.8f + fForceAdd,0), vPoint);
		}

		if(mpCharBody->mpCallback)
			mpCharBody->mpCallback->OnGravityCollide(mpCharBody,apBody,apCollideData);
	}

	//-----------------------------------------------------------------------

	//////////////////////////////////////////////////////////////////////////
	// COLLIDE PUSH CALLBACK
	//////////////////////////////////////////////////////////////////////////

	//-----------------------------------------------------------------------

	cCharacterBodyCollidePush::cCharacterBodyCollidePush()
	{
		mpCharBody = NULL;
	}

	//-----------------------------------------------------------------------

	void cCharacterBodyCollidePush::OnCollision(iPhysicsBody *apBody, cCollideData *apCollideData)
	{
		//No pushing if the player is not moving
		if(mpCharBody->GetMoveSpeed(eCharDir_Forward)==0 &&
			mpCharBody->GetMoveSpeed(eCharDir_Right)==0) return;

		//Check what bodies not to push.
		if(apBody->GetMass()==0 || apBody->GetMass() > mpCharBody->GetMaxPushMass()) return;

		bool bPush=false;
		cVector3f vPoint(0,0,0);
		float fNumPoints=0;


		//Go through all of the contact points
		for(int i=0; i< apCollideData->mlNumOfPoints; i++)
		{
			cCollidePoint &point = apCollideData->mvContactPoints[i];

			bPush = true;
			fNumPoints+=1;
			vPoint += point.mvPoint;
		}

		if(bPush)
		{
			vPoint = vPoint / fNumPoints;

			if(mpCharBody->GetPushIn2D())
			{
				cVector3f vDir = apBody->GetWorldPosition() - mpCharBody->GetPosition();
				vDir.y =0; vDir.Normalise();

				apBody->AddForceAtPosition(vDir * mpCharBody->GetPushForce(), vPoint);
			}
			else
			{
				cVector3f vDir = cMath::Vector3Normalize(apBody->GetWorldPosition() - mpCharBody->GetPosition());

				apBody->AddForceAtPosition(vDir * mpCharBody->GetPushForce(), vPoint);
			}


		}

	}

	//-----------------------------------------------------------------------

	//////////////////////////////////////////////////////////////////////////
	// PUBLIC METHODS
	//////////////////////////////////////////////////////////////////////////

	//-----------------------------------------------------------------------

	float iCharacterBody::GetMass()
	{
		return mfMass;
	}
	void iCharacterBody::SetMass(float afMass)
	{
		mfMass = afMass;
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::SetActive(bool abX)
	{
		if(mbActive == abX) return;

		mbActive = abX;

		for(size_t i=0; i < mvExtraBodies.size(); ++i)
		{
			if(mvExtraBodies[i]){
				mvExtraBodies[i]->SetActive(mbActive);
			}
		}

	}

	//-----------------------------------------------------------------------

	void iCharacterBody::SetCollideCharacter(bool abX)
	{
		if(abX == mbCollideCharacter) return;

		mbCollideCharacter = abX;

		for(size_t i=0; i < mvExtraBodies.size(); ++i)
		{
			if(mvExtraBodies[i]){
				mvExtraBodies[i]->SetCollideCharacter(mbCollideCharacter);
			}
		}
	}

	//-----------------------------------------------------------------------

	cVector3f iCharacterBody::GetSize()
	{
		return mvSize;
	}

	//-----------------------------------------------------------------------

	int iCharacterBody::AddExtraSize(const cVector3f& avSize)
	{
		float fRadius = cMath::Max(avSize.x, avSize.z)*0.5f;
		cMatrixf mtxOffset = cMath::MatrixRotateZ(kPi2f);
		iCollideShape *pCollider = mpWorld->CreateCylinderShape(fRadius, avSize.y,&mtxOffset);
		//iCollideShape *pCollider = mpWorld->CreateBoxShape(avSize,NULL);

		iPhysicsBody *pBody = mpWorld->CreateBody(msName,pCollider);
		pBody->SetMass(0);
		pBody->SetGravity(false);
		pBody->SetIsCharacter(true);
		pBody->SetActive(false);
		pBody->SetCharacterBody(this);

		mvExtraBodies.push_back(pBody);

		return (int)mvExtraBodies.size() -1;
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::SetActiveSize(int alNum)
	{
		mpBody->SetActive(false);
		mpBody = mvExtraBodies[alNum];
		mpBody->SetActive(true);

		//Set size of the new body.
		mvSize.y = mpBody->GetShape()->GetHeight();
		mvSize.x = mpBody->GetShape()->GetRadius() * 2;
		mvSize.z = mpBody->GetShape()->GetRadius() * 2;
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::SetMaxPositiveMoveSpeed(eCharDir aDir, float afX)
	{
		mfMaxPosMoveSpeed[aDir] = afX;
	}
	float iCharacterBody::GetMaxPositiveMoveSpeed(eCharDir aDir)
	{
		return mfMaxPosMoveSpeed[aDir];
	}
	void iCharacterBody::SetMaxNegativeMoveSpeed(eCharDir aDir, float afX)
	{
		mfMaxNegMoveSpeed[aDir] = afX;
	}
	float iCharacterBody::GetMaxNegativeMoveSpeed(eCharDir aDir)
	{
		return mfMaxNegMoveSpeed[aDir];
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::SetMoveSpeed(eCharDir aDir, float afX)
	{
		mfMoveSpeed[aDir] = afX;
	}
	float iCharacterBody::GetMoveSpeed(eCharDir aDir)
	{
		return mfMoveSpeed[aDir];
	}

	void iCharacterBody::SetMoveAcc(eCharDir aDir, float afX)
	{
		mfMoveAcc[aDir] = afX;
	}
	float iCharacterBody::GetMoveAcc(eCharDir aDir)
	{
		return mfMoveAcc[aDir];
	}

	void iCharacterBody::SetMoveDeacc(eCharDir aDir, float afX)
	{
		mfMoveDeacc[aDir] = afX;
	}
	float iCharacterBody::GetMoveDeacc(eCharDir aDir)
	{
		return mfMoveDeacc[aDir];
	}

	//-----------------------------------------------------------------------

	cVector3f iCharacterBody::GetVelocity(float afFrameTime)
	{
		if(afFrameTime <=0) return 0;

		return (mvPosition - mvLastPosition) / afFrameTime;
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::SetPosition(const cVector3f& avPos, bool abSmooth)
	{
		mvForce =0;
		mvVelolcity =0;
		mvLastPosition = avPos;
		mvPosition = avPos;
		mpBody->SetPosition(avPos);

		if(!abSmooth)mlstCameraPos.clear();
	}
	const cVector3f& iCharacterBody::GetPosition()
	{
		return mvPosition;
	}
	const cVector3f& iCharacterBody::GetLastPosition()
	{
		return mvLastPosition;
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::SetFeetPosition(const cVector3f& avPos, bool abSmooth)
	{
		SetPosition(avPos + cVector3f(0,mpBody->GetShape()->GetSize().y/2,0),abSmooth);
	}

	cVector3f iCharacterBody::GetFeetPosition()
	{
		return mvPosition - cVector3f(0,mpBody->GetShape()->GetSize().y/2,0);
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::SetYaw(float afX)
	{
		mfYaw = afX;
	}

	float iCharacterBody::GetYaw()
	{
		return mfYaw;
	}

	void iCharacterBody::AddYaw(float afX)
	{
		mfYaw += afX;
	}

	void iCharacterBody::SetPitch(float afX)
	{
		mfPitch = afX;
	}

	void iCharacterBody::AddPitch(float afX)
	{
		mfPitch += afX;
	}

	float iCharacterBody::GetPitch()
	{
		return mfPitch;
	}

	//-----------------------------------------------------------------------

	cVector3f iCharacterBody::GetForward()
	{
		return m_mtxMove.GetForward()*-1.0f;
	}
	cVector3f iCharacterBody::GetRight()
	{
		return m_mtxMove.GetRight();
	}
	cVector3f iCharacterBody::GetUp()
	{
		return m_mtxMove.GetUp();
	}

	//-----------------------------------------------------------------------

	cMatrixf& iCharacterBody::GetMoveMatrix()
	{
		return m_mtxMove;
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::SetGravityActive(bool abX)
	{
		mbGravityActive = abX;
	}

	bool iCharacterBody::GravityIsActive()
	{
		return mbGravityActive;
	}

	void iCharacterBody::SetMaxGravitySpeed(float afX)
	{
		mfMaxGravitySpeed = afX;
	}
	float iCharacterBody::GetMaxGravitySpeed()
	{
		return mfMaxGravitySpeed;
	}

	//-----------------------------------------------------------------------

	bool iCharacterBody::GetCustomGravityActive()
	{
		return mbCustomGravity;
	}

	void iCharacterBody::SetCustomGravityActive(bool abX)
	{
		mbCustomGravity = abX;
	}

	void iCharacterBody::SetCustomGravity(const cVector3f& avCustomGravity)
	{
		mvCustomGravity = avCustomGravity;
	}

	cVector3f iCharacterBody::GetCustomGravity()
	{
		return mvCustomGravity;
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::SetForce(const cVector3f &avForce)
	{
		mvForce = avForce;
	}

	void iCharacterBody::AddForce(const cVector3f &avForce)
	{
		mvForce += avForce;
	}

	cVector3f iCharacterBody::GetForce()
	{
		return mvForce;
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::Move(eCharDir aDir, float afMul, float afTimeStep)
	{
		mfMoveSpeed[aDir] += mfMoveAcc[aDir]*afMul*afTimeStep;

		mbMoving[aDir] = true;

		//Clamp the speed.
		mfMoveSpeed[aDir] = cMath::Clamp(mfMoveSpeed[aDir],mfMaxNegMoveSpeed[aDir],mfMaxPosMoveSpeed[aDir]);
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::Update(float afTimeStep)
	{
		if(mbActive == false) return;

		//Clear attached body
		iPhysicsBody *pLastAttached = mpAttachedBody;
		SetAttachedBody(NULL);

		//Update the move matrix.
		UpdateMoveMarix();

		mvLastPosition = mvPosition;

		/////////////////////////////
		// Ground and air friction:
		float fFriction = mbOnGround ? mfGroundFriction : mfAirFriction;

		//This is not working for some reason... Probably because it thinks its on the ground when it is really in the air.
		/*if(mbOnGround)
		{
			float fSpeed = mvVelolcity.Length();
			cVector3f vDir = cMath::Vector3Normalize(mvVelolcity);

			fSpeed -= fFriction * afTimeStep;
			if(fSpeed<0) fSpeed=0;

			mvVelolcity = vDir * fSpeed;
		}
		else*/
		{
			cVector3f vVelXZ(mvVelolcity.x, 0,mvVelolcity.z);
			float fSpeed = vVelXZ.Length();
			vVelXZ.Normalise();

			fSpeed -= fFriction * afTimeStep;
			if(fSpeed<0) fSpeed=0;

			mvVelolcity.x = vVelXZ.x * fSpeed;
			mvVelolcity.z = vVelXZ.z * fSpeed;
		}


		////////////////////////////////
		//Set the postion to the body
		mpBody->SetPosition(mvPosition);

		//////////////////////////
		//Enable objects around the character
		if(mvLastPosition != mvPosition)
		{
			cBoundingVolume largeBV = *mpBody->GetBV();
			largeBV.SetSize(largeBV.GetSize()*1.02f);

			mpWorld->EnableBodiesInBV(&largeBV,true);
		}



		/////////////////////////////////////////////////////
		// If the character is not moving in a direction, apply deacceleration.
		for(int i=0; i < eCharDir_LastEnum; i++)
		{
			if(mbMoving[i]==false)
			{
				if(mfMoveSpeed[i]>0)
				{
					mfMoveSpeed[i] -= mfMoveDeacc[i] * afTimeStep;
					if(mfMoveSpeed[i]<0)mfMoveSpeed[i] =0;
				}
				else
				{
					mfMoveSpeed[i] += mfMoveDeacc[i] * afTimeStep;
					if(mfMoveSpeed[i]>0)mfMoveSpeed[i] =0;
				}
			}
			else
			{
				mbMoving[i] = false;
			}
		}

		/////////////////////////////////////////
		//Update postion
		if(mbClimbing)	mfCheckStepClimbCount =0;
		else			mfCheckStepClimbCount -= afTimeStep;

		mbClimbing = false;

		cVector3f vPosAdd(0,0,0);

		//Movement velocity
		cVector3f vForward = GetMoveMatrix().GetForward() *-1.0f;
		cVector3f vRight = GetMoveMatrix().GetRight();

		vPosAdd += vForward * mfMoveSpeed[eCharDir_Forward] * afTimeStep;
		vPosAdd += vRight * mfMoveSpeed[eCharDir_Right] * afTimeStep;

		//Make sure speed is not greate than max forward.
		float fMaxStep =	mfMoveSpeed[eCharDir_Forward] >=0 ?
							mfMaxPosMoveSpeed[eCharDir_Forward] :
							mfMaxNegMoveSpeed[eCharDir_Forward];
		fMaxStep *= afTimeStep;
		fMaxStep = cMath::Abs(fMaxStep);

		float fStepLength = vPosAdd.Length();
		if(fStepLength > fMaxStep){
			vPosAdd = vPosAdd / fStepLength;
			vPosAdd = vPosAdd * fMaxStep;
		}

		//Check if collision should be tested
		if(mbTestCollision==false)
		{
			mpBody->SetPosition(mvPosition);

			UpdateCamera();
			UpdateEntity();

			mvVelolcity =0; //This is a fix so the velocity is not screwed up later on.

			return;
		}

		/////////////////////////////////////
		//Check for collision.
		//Might wanna test this for x, y and z independently.
		mvPosition += vPosAdd;

		cVector3f vNewPos;
		//vNewPos = mvPosition;
		if(mvLastPosition.x != mvPosition.x || mvLastPosition.z != mvLastPosition.z)
		{
			mpWorld->CheckShapeWorldCollision(&vNewPos,mpBody->GetShape(), cMath::MatrixTranslate(mvPosition),mpBody,
											false, true, mpCollideCallbackPush,mbCollideCharacter);
		}
		else
		{
			vNewPos = mvPosition;
		}

		//If the player is halted, check if there might be a step infront of him.
		if(	(mvPosition.x != vNewPos.x || mvPosition.z != vNewPos.z ||
			(mbGravityActive==false && mvPosition.y != vNewPos.y) ) )
		{
			mvPosition = vNewPos;

			//Log("--- Colliding ----!\n");

			if(mfCheckStepClimbCount <= 0)
			{
				//Send a ray in front of the player.
				cVector3f vShapeSize = mpBody->GetShape()->GetSize();
				float fRadius = mpBody->GetShape()->GetRadius();
				float fForwadAdd = vPosAdd.Length();

				//The direction of the movement.
				cVector3f vMoveDir = cMath::Vector3Normalize(vPosAdd);

				//Log("MoveDir: %s\n",vMoveDir.ToString().c_str());

				cVector3f vStepAdd[3];
				cVector3f vStart[3];
				cVector3f vEnd[3];
				bool bCollided[3];
				float fMinDist[3];
				int lNumRays= 1;
				if(mbAccurateClimbing) lNumRays = 3;

				/////////////////////////////////
				//Calculate the different movements
				vStepAdd[0] = vMoveDir*(fRadius+fForwadAdd);

				if(mbAccurateClimbing)
				{
					cVector3f vRightDir = cMath::MatrixMul(cMath::MatrixRotateY(kPi4f),vMoveDir);
					vStepAdd[1] = (vRightDir*fRadius)+(vMoveDir*fForwadAdd);

					cVector3f vLeftDir = cMath::MatrixMul(cMath::MatrixRotateY(-kPi4f),vMoveDir);
					vStepAdd[2] = (vLeftDir*fRadius)+(vMoveDir*fForwadAdd);
				}

				/////////////////////////////////
				//Shot the rays
				for(int i=0; i< lNumRays; ++i)
				{
					vStart[i] = mvPosition + cVector3f(0,vShapeSize.y/2,0)+ vStepAdd[i];
					vEnd[i] = vStart[i] - cVector3f(0,vShapeSize.y,0);

					mpRayCallback->Clear();
					mpWorld->CastRay(mpRayCallback,vStart[i],vEnd[i],true,false,false);
					bCollided[i] = mpRayCallback->mbCollide;
					fMinDist[i] = mpRayCallback->mfMinDist;
				}

				/////////////////////////////////7
				// Check if the step can be climbed.
				for(int i=0; i< lNumRays; ++i)
				{
					if(bCollided[i]==false) continue;
					//Log("Ray collided!\n");

					float fHeight = vShapeSize.y - fMinDist[i];

					//Log("Height: %f MinDist: %f\n",fHeight,mpRayCallback->mfMinDist);

					if(fHeight <= mfMaxStepHeight)
					{
						//Log("Trying to step up!\n");
						//Check if there is any collision on the new pos
						cVector3f vStepPos = mvPosition + cVector3f(0,fHeight+mfClimbHeightAdd,0)+
							(vMoveDir*fForwadAdd*mfClimbForwardMul);

						mpWorld->CheckShapeWorldCollision(&vNewPos,mpBody->GetShape(),
							cMath::MatrixTranslate(vStepPos),mpBody,
							false, true,NULL,mbCollideCharacter);
						//Log("Old pos: (%s) StepPos: (%s) NewPos: (%s)\n",mvPosition.ToString().c_str(),
						//							vStepPos.ToString().c_str(),vNewPos.ToString().c_str());
						if(vNewPos == vStepPos)
						{
							//mvPosition = vStepPos;
							//Climb the stair.
							mvPosition.y += mfStepClimbSpeed * afTimeStep;
							mbClimbing = true;
							break;
						}
					}
				}

				mfCheckStepClimbCount = mfCheckStepClimbInterval;
			}
		}


		if(mbGravityActive==false){
			//Set the postion to the body
			mpBody->SetPosition(mvPosition);

			UpdateCamera();
			UpdateEntity();

			mvVelolcity =0; //This is a fix so the velocity is not screwed up later on. (still is though...)

			return;
		}

		/////////////////////////////////////
		//Update External forces
		//Log(" vel1: %s\n",mvVelolcity.ToString().c_str());
		cVector3f vBeforeForcePos = mvPosition;

		mvVelolcity += mvForce * (afTimeStep * (1.0f/mfMass));
		if(mbGravityActive && mbClimbing==false)
		{
			if(mbCustomGravity)
				mvVelolcity += mvCustomGravity * afTimeStep;
			else
				mvVelolcity += mpWorld->GetGravity() * afTimeStep;

			float fLength = mvVelolcity.Length();
			if(fLength> mfMaxGravitySpeed)
			{
				mvVelolcity = (mvVelolcity /fLength) *mfMaxGravitySpeed;
			}
		}

		//Log(" vel2: %s\n",mvVelolcity.ToString().c_str());

		mvForce = cVector3f(0,0,0);

		cVector3f vLastVelocity(0,0,0);

		////////////////////////////
		// Check x and z collision
		if(mvVelolcity.x !=0 || mvVelolcity.z!=0)
		{
			mvPosition += cVector3f(mvVelolcity.x, 0, mvVelolcity.z) * afTimeStep;

			vNewPos = mvPosition;
			bool bCollide = mpWorld->CheckShapeWorldCollision(&vNewPos,mpBody->GetShape(), cMath::MatrixTranslate(mvPosition),
												mpBody,false,true,NULL,mbCollideCharacter);

			//Set new velocity depending on collisions.
			vLastVelocity.x = mvVelolcity.x;
			vLastVelocity.z = mvVelolcity.z;

			mvVelolcity.x = (vNewPos.x - vBeforeForcePos.x) * (1/afTimeStep);
			mvVelolcity.z = (vNewPos.z - vBeforeForcePos.z) * (1/afTimeStep);

			//Make sure the speed doesn't change direction.
			//if((vLastVelocity.x<0 && mvVelolcity.x>0) || (vLastVelocity.x>0 && mvVelolcity.x<0)) mvVelolcity.x =0;
			//if((vLastVelocity.z<0 && mvVelolcity.z>0) || (vLastVelocity.z>0 && mvVelolcity.z<0)) mvVelolcity.z =0;


			mvPosition = vNewPos;
			//Log("Some strange force!!\n");
		}

		//if(mbGravityActive==false)return;
		//Log("Before yforce: %s\n",mvPosition.ToString().c_str());


		////////////////////////////////
		// Check y collision, this is to be sure if the character touches the ground.
		bool bCollide = false;

		//Log("VelY 2: %f\n", mvVelolcity.y);

		cVector3f vPosbeforeGrav = mvPosition;
		mvPosition.y += mvVelolcity.y * afTimeStep;


		vNewPos = mvPosition;
		bCollide = mpWorld->CheckShapeWorldCollision(&vNewPos,mpBody->GetShape(),
											cMath::MatrixTranslate(mvPosition), mpBody,false,true,
											mpCollideCallbackGravity,mbCollideCharacter);


		//Set new velocity depending on collisions.
		vLastVelocity.y = mvVelolcity.y;

		//Log(" vel4: %s\n",mvVelolcity.ToString().c_str());

		//If climbing we don't wanna leave behind a great up speed.
		if(mbClimbing)
		{
			if(mvVelolcity.y<0)
				mvVelolcity.y = 0;
		}
		else
		{
			//mvVelolcity.y = (vNewPos.y - vBeforeForcePos.y) * (1/afTimeStep);
			mvVelolcity.y = (vNewPos.y - vPosbeforeGrav.y) * (1/afTimeStep);

			//This s a fix that I am not 100% sure is good.
			if(vLastVelocity.y < 0 && mvVelolcity.y > 0) mvVelolcity.y =0;
			if(vLastVelocity.y > 0 && mvVelolcity.y < 0) mvVelolcity.y =0;
		}

		//Log(" vel5: %s\n",mvVelolcity.ToString().c_str());

		//Log("After Velocity: %s\n--------\n",mvVelolcity.ToString().c_str());
		//Make sure the speed doesn't change direction.Should not be needed.
		//if((vLastVelocity.y<0 && mvVelolcity.y>0) || (vLastVelocity.y>0 && mvVelolcity.y<0)) mvVelolcity.y =0;

		mvPosition = vNewPos;

		///////////////////////////////////////////////
		//Determine if character is on the ground.
		if(mbClimbing)
		{
			//if climbing there is no gravity working but we still want foot steps.
			mbOnGround = true;
		}
		else
		{
			if(bCollide && vLastVelocity.y<=0 && std::abs(mvVelolcity.y) < (std::abs(vLastVelocity.y)-0.001f))
			{
				if(mbOnGround == false)
				{
					if(mpCallback) mpCallback->OnHitGround(this,vLastVelocity);
				}

				mbOnGround = true;
			}
			else
			{
				mbOnGround = false;
			}
		}

		//This is so that the character does not slide down on
		//small slopes.
		if(mbOnGround && mvVelolcity.y<0 && mvVelolcity.y > -0.15)
		{
			mvPosition = vPosbeforeGrav;
		}


		//debug:
		//mbOnGround = true;

		//////////////////////////////
		//Update attached objects
		UpdateCamera();
		UpdateEntity();

		////////////////////////////
		//Update body if it is attached
		if(pLastAttached != mpAttachedBody)	mbAttachmentJustAdded = true;
		UpdateAttachment();

		//////////////////////////////
		//Enable close by objects

		if(mbEnableNearbyBodies)
		{
			cWorld3D *pWorld = mpWorld->GetWorld3D();
			cPortalContainerEntityIterator bodyIt = pWorld->GetPortalContainer()->GetEntityIterator(
															mpBody->GetBoundingVolume());

			cBoundingVolume bv = *mpBody->GetBV();
			bv.SetLocalMinMax(bv.GetLocalMin() - cVector3f(0.03f,0.03f,0.03f),bv.GetLocalMax() + cVector3f(0.03f,0.03f,0.03f));
			float fHeadY = mpBody->GetWorldPosition().y + mpBody->GetShape()->GetSize().y/2 - 0.01f;
			//float fBVHeight = 0.1;
			//bv.SetPosition(mpBody->GetWorldPosition().y + fBVHeight/2);
			//bv.SetSize( cVector3f(mpBody->GetShape()->GetSize().x*0.9f, fBVHeight, mpBody->GetShape()->GetSize().z*0.9f) );

			while(bodyIt.HasNext())
			{
				iPhysicsBody *pBody = static_cast<iPhysicsBody*>(bodyIt.Next());

				if(	pBody->IsActive() &&
					fHeadY <= pBody->GetLocalPosition().y &&
					pBody->GetEnabled()==false &&
					cMath::CheckCollisionBV(*pBody->GetBV(), bv))
				{
					pBody->SetEnabled(true);
				}
			}
		}
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::SetCamera(cCamera3D *apCam)
	{
		mpCamera = apCam;
	}

	cCamera3D* iCharacterBody::GetCamera()
	{
		return mpCamera;
	}

	void iCharacterBody::SetCameraPosAdd(const cVector3f &avAdd)
	{
		mvCameraPosAdd = avAdd;
	}
	cVector3f iCharacterBody::GetCameraPosAdd()
	{
		return mvCameraPosAdd;
	}

	//-----------------------------------------------------------------------

	iCollideShape * iCharacterBody::GetShape()
	{
		return mpBody->GetShape();
	}

	//-----------------------------------------------------------------------

	bool iCharacterBody::IsOnGround()
	{
		return mbOnGround;
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::SetEntity(iEntity3D *apEntity)
	{
		mpEntity = apEntity;
	}
	iEntity3D* iCharacterBody::GetEntity()
	{
		return mpEntity;
	}
	void iCharacterBody::SetEntityOffset(const cMatrixf &a_mtxOffset)
	{
		m_mtxEntityOffset = a_mtxOffset;
	}
	const cMatrixf& iCharacterBody::GetEntityOffset()
	{
		return m_mtxEntityOffset;
	}

	void iCharacterBody::SetEntityPostOffset(const cMatrixf &a_mtxOffset)
	{
		m_mtxEntityPostOffset = a_mtxOffset;
	}
	const cMatrixf& iCharacterBody::GetEntityPostOffset()
	{
		return m_mtxEntityPostOffset;
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::SetAttachedBody(iPhysicsBody *apBody)
	{
		if(apBody == mpAttachedBody) return;

		if(mpAttachedBody) mpAttachedBody->RemoveAttachedCharacter(this);

		mpAttachedBody = apBody;
		if(mpAttachedBody) mpAttachedBody->AddAttachedCharacter(this);
	}

	//-----------------------------------------------------------------------


	//////////////////////////////////////////////////////////////////////////
	// PRIVATE METHODS
	//////////////////////////////////////////////////////////////////////////

	//-----------------------------------------------------------------------
	void iCharacterBody::UpdateMoveMarix()
	{
		m_mtxMove = cMath::MatrixRotateY(-mfYaw);
		m_mtxMove = cMath::MatrixMul(cMath::MatrixRotateX(-mfPitch),m_mtxMove);
		m_mtxMove.SetTranslation(mvPosition * -1);
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::UpdateCamera()
	{
		if(mpCamera==NULL) return;

		if(mlCameraSmoothPosNum <=0)
		{
			cVector3f vPos = (mvPosition - cVector3f(0,mpBody->GetShape()->GetSize().y/2.0f,0)) +
							cVector3f(0,mvSize.y,0);
			mpCamera->SetPosition(vPos + mvCameraPosAdd);
		}
		//Smooth the camera position
		else
		{
			//Add the newest position.
			mlstCameraPos.push_back(mvPosition);
			//If to too large remove the oldest.
			if((int)mlstCameraPos.size() > mlCameraSmoothPosNum)
			{
				mlstCameraPos.erase(mlstCameraPos.begin());
			}

			float fNum = (float)mlstCameraPos.size();

			//Add all positions and devide by the number of em.
			//that way we get the average
			cVector3f vTotalPos(0,0,0);
			tVector3fListIt it=mlstCameraPos.begin();
			for(; it!= mlstCameraPos.end(); ++it)
			{
				vTotalPos += *it;
			}

			cVector3f vPos = vTotalPos / fNum;
			cVector3f vFirstSize = mvExtraBodies[0]->GetShape()->GetSize();

			cVector3f vHeadPos = (vPos - cVector3f(0,mpBody->GetShape()->GetSize().y/2.0f,0)) +
								cVector3f(0,vFirstSize.y,0);

			mpCamera->SetPosition(vHeadPos + mvCameraPosAdd);
		}
	}

	//-----------------------------------------------------------------------


	void iCharacterBody::UpdateEntity()
	{
		if(mpEntity==NULL) return;

		if(mlEntitySmoothPosNum <=0)
		{
			cMatrixf mtxEntity = cMath::MatrixRotateY(mfYaw);
			mtxEntity.SetTranslation(mvPosition);

			mpEntity->SetMatrix(cMath::MatrixMul(mtxEntity,m_mtxEntityOffset));
		}
		//Smooth the camera position
		else
		{
			//Add the newest position.
			mlstEntityPos.push_back(mvPosition);
			//If to too large remove the oldest.
			if((int)mlstEntityPos.size() > mlEntitySmoothPosNum)
			{
				mlstEntityPos.erase(mlstEntityPos.begin());
			}

			float fNum = (float)mlstEntityPos.size();

			//Add all positions and devide by the number of em.
			//that way we get the average
			cVector3f vTotalPos(0,0,0);
			tVector3fListIt it=mlstEntityPos.begin();
			for(; it!= mlstEntityPos.end(); ++it)
			{
				vTotalPos += *it;
			}

			cVector3f vPos = vTotalPos / fNum;

			cMatrixf mtxEntity = cMath::MatrixInverse(m_mtxMove);
			mtxEntity.SetTranslation(0);

			mtxEntity = cMath::MatrixMul(m_mtxEntityPostOffset,mtxEntity);
			mtxEntity.SetTranslation(mtxEntity.GetTranslation() + vPos);

			mtxEntity = cMath::MatrixMul(mtxEntity,m_mtxEntityOffset);

			mpEntity->SetMatrix(mtxEntity);
		}
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::UpdateAttachment()
	{
		if(mpAttachedBody==NULL){
			mbAttachmentJustAdded = true;
			return;
		}

		//Log("Updateattachment %d\n",mbAttachmentJustAdded);

		if(mbAttachmentJustAdded)
		{
			mbAttachmentJustAdded = false;
		}
		else
		{
			cVector3f vPosAdd = mpAttachedBody->GetWorldPosition() -
								m_mtxAttachedPrevMatrix.GetTranslation();

			mvPosition += vPosAdd;
		}

		m_mtxAttachedPrevMatrix = mpAttachedBody->GetWorldMatrix();
	}

	//-----------------------------------------------------------------------

	//////////////////////////////////////////////////////////////////////////
	// SAVE OBJECT STUFF
	//////////////////////////////////////////////////////////////////////////

	//-----------------------------------------------------------------------

	kBeginSerialize(cSaveData_iCharacterBody,iSaveData)
	kSerializeVar(msName,eSerializeType_String)

	kSerializeVar(mfMass,eSerializeType_Float32)
	kSerializeVar(mbGravityActive, eSerializeType_Bool)
	kSerializeVar(mfMaxGravitySpeed,eSerializeType_Float32)

	kSerializeVar(mbActive,eSerializeType_Bool)

	kSerializeVar(mbCollideCharacter,eSerializeType_Bool)

	kSerializeVar(mvPosition,eSerializeType_Vector3f)
	kSerializeVar(mvLastPosition,eSerializeType_Vector3f)
	kSerializeVarArray(mfMaxPosMoveSpeed,eSerializeType_Float32,2)
	kSerializeVarArray(mfMaxNegMoveSpeed,eSerializeType_Float32,2)
	kSerializeVarArray(mfMoveSpeed,eSerializeType_Float32,2)
	kSerializeVarArray(mfMoveAcc,eSerializeType_Float32,2)
	kSerializeVarArray(mfMoveDeacc,eSerializeType_Float32,2)
	kSerializeVarArray(mbMoving,eSerializeType_Bool,2)

	kSerializeVar(mfPitch,eSerializeType_Float32)
	kSerializeVar(mfYaw,eSerializeType_Float32)

	kSerializeVar(mbOnGround,eSerializeType_Bool)

	kSerializeVar(mfMaxPushMass,eSerializeType_Float32)
	kSerializeVar(mfPushForce,eSerializeType_Float32)

	kSerializeVar(mvForce,eSerializeType_Vector3f)
	kSerializeVar(mvVelolcity,eSerializeType_Vector3f)

	kSerializeVar(mvSize,eSerializeType_Vector3f)

	kSerializeVar(m_mtxMove, eSerializeType_Matrixf)

	kSerializeVar(mlEntityId,eSerializeType_Int32)
	kSerializeVar(m_mtxEntityOffset, eSerializeType_Matrixf)
	kSerializeVar(mlEntitySmoothPosNum,eSerializeType_Int32)

	kSerializeVar(mfMaxStepHeight,eSerializeType_Float32)
	kSerializeVar(mfStepClimbSpeed,eSerializeType_Float32)
	kSerializeVar(mfClimbForwardMul,eSerializeType_Float32)
	kSerializeVar(mfClimbHeightAdd, eSerializeType_Float32)
	kSerializeVar(mbClimbing,eSerializeType_Bool)

	kSerializeVar(mfGroundFriction,eSerializeType_Float32)

	kSerializeVar(mlBodyId,eSerializeType_Int32)

	kSerializeVarContainer(mvExtraBodyIds,eSerializeType_Int32)
	kEndSerialize()

	//-----------------------------------------------------------------------

	iSaveObject* cSaveData_iCharacterBody::CreateSaveObject(cSaveObjectHandler *apSaveObjectHandler,cGame *apGame)
	{
		iPhysicsWorld *pWorld = apGame->GetScene()->GetWorld3D()->GetPhysicsWorld();

		iCharacterBody *pBody = pWorld->CreateCharacterBody(msName,mvSize);

		//Destroy the recently created body.
		pWorld->DestroyBody(pBody->mpBody);
		pBody->mvExtraBodies.clear();

		return pBody;
	}

	//-----------------------------------------------------------------------

	int cSaveData_iCharacterBody::GetSaveCreatePrio()
	{
		return 1;
	}

	//-----------------------------------------------------------------------

	iSaveData* iCharacterBody::CreateSaveData()
	{
		return hplNew( cSaveData_iCharacterBody, () );
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::SaveToSaveData(iSaveData *apSaveData)
	{
		kSaveData_SaveToBegin(iCharacterBody);

		//////////////////////////
		//Variables
		kSaveData_SaveTo(msName);

		kSaveData_SaveTo(mfMass);
		kSaveData_SaveTo(mbGravityActive);
		kSaveData_SaveTo(mfMaxGravitySpeed);

		kSaveData_SaveTo(mbActive);

		kSaveData_SaveTo(mbCollideCharacter);

		kSaveData_SaveTo(mvPosition);
		kSaveData_SaveTo(mvLastPosition);

		kSaveData_SaveTo(mfMaxPosMoveSpeed[0]);
		kSaveData_SaveTo(mfMaxNegMoveSpeed[0]);
		kSaveData_SaveTo(mfMoveSpeed[0]);
		kSaveData_SaveTo(mfMoveAcc[0]);
		kSaveData_SaveTo(mfMoveDeacc[0]);
		kSaveData_SaveTo(mbMoving[0]);

		kSaveData_SaveTo(mfMaxPosMoveSpeed[1]);
		kSaveData_SaveTo(mfMaxNegMoveSpeed[1]);
		kSaveData_SaveTo(mfMoveSpeed[1]);
		kSaveData_SaveTo(mfMoveAcc[1]);
		kSaveData_SaveTo(mfMoveDeacc[1]);
		kSaveData_SaveTo(mbMoving[1]);

		kSaveData_SaveTo(mfPitch);
		kSaveData_SaveTo(mfYaw);

		kSaveData_SaveTo(mbOnGround);

		kSaveData_SaveTo(mfMaxPushMass);
		kSaveData_SaveTo(mfPushForce);

		kSaveData_SaveTo(mvForce);
		kSaveData_SaveTo(mvVelolcity);

		kSaveData_SaveTo(mvSize);

		kSaveData_SaveTo(m_mtxMove);

		kSaveData_SaveTo(m_mtxEntityOffset);
		kSaveData_SaveTo(mlEntitySmoothPosNum);

		kSaveData_SaveTo(mfMaxStepHeight);
		kSaveData_SaveTo(mfStepClimbSpeed);
		kSaveData_SaveTo(mfClimbForwardMul);
		kSaveData_SaveTo(mfClimbHeightAdd);
		kSaveData_SaveTo(mbClimbing);

		kSaveData_SaveTo(mfGroundFriction);


		//////////////////////////
		//Containers


		//////////////////////////
		//Pointers
		kSaveData_SaveObject(mpEntity,mlEntityId);
		kSaveData_SaveObject(mpBody,mlBodyId);
		kSaveData_SaveIdList(mvExtraBodies,std::vector<iPhysicsBody*>::iterator,mvExtraBodyIds);
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::LoadFromSaveData(iSaveData *apSaveData)
	{
		kSaveData_LoadFromBegin(iCharacterBody);

		//////////////////////////
		//Variables
		kSaveData_LoadFrom(msName);

		kSaveData_LoadFrom(mfMass);
		kSaveData_LoadFrom(mbGravityActive);
		kSaveData_LoadFrom(mfMaxGravitySpeed);

		kSaveData_LoadFrom(mbActive);

		kSaveData_LoadFrom(mbCollideCharacter);

		kSaveData_LoadFrom(mvPosition);
		kSaveData_LoadFrom(mvLastPosition);

		kSaveData_LoadFrom(mfMaxPosMoveSpeed[0]);
		kSaveData_LoadFrom(mfMaxNegMoveSpeed[0]);
		kSaveData_LoadFrom(mfMoveSpeed[0]);
		kSaveData_LoadFrom(mfMoveAcc[0]);
		kSaveData_LoadFrom(mfMoveDeacc[0]);
		kSaveData_LoadFrom(mbMoving[0]);

		kSaveData_LoadFrom(mfMaxPosMoveSpeed[1]);
		kSaveData_LoadFrom(mfMaxNegMoveSpeed[1]);
		kSaveData_LoadFrom(mfMoveSpeed[1]);
		kSaveData_LoadFrom(mfMoveAcc[1]);
		kSaveData_LoadFrom(mfMoveDeacc[1]);
		kSaveData_LoadFrom(mbMoving[1]);

		kSaveData_LoadFrom(mfPitch);
		kSaveData_LoadFrom(mfYaw);

		kSaveData_LoadFrom(mbOnGround);

		kSaveData_LoadFrom(mfMaxPushMass);
		kSaveData_LoadFrom(mfPushForce);

		kSaveData_LoadFrom(mvForce);
		kSaveData_LoadFrom(mvVelolcity);

		kSaveData_LoadFrom(mvSize);

		kSaveData_LoadFrom(m_mtxMove);

		kSaveData_LoadFrom(m_mtxEntityOffset);
		kSaveData_LoadFrom(mlEntitySmoothPosNum);

		kSaveData_LoadFrom(mfMaxStepHeight);
		kSaveData_LoadFrom(mfStepClimbSpeed);
		kSaveData_LoadFrom(mfClimbForwardMul);
		kSaveData_LoadFrom(mfClimbHeightAdd);
		kSaveData_LoadFrom(mbClimbing);

		kSaveData_LoadFrom(mfGroundFriction);
	}

	//-----------------------------------------------------------------------

	void iCharacterBody::SaveDataSetup(cSaveObjectHandler *apSaveObjectHandler, cGame *apGame)
	{
		kSaveData_SetupBegin(iCharacterBody);

		//////////////////////////
		//Pointers
		kSaveData_LoadObject(mpEntity,mlEntityId,iEntity3D*);
		kSaveData_LoadObject(mpBody,mlBodyId,iPhysicsBody*);
		//kSaveData_LoadIdList(mvExtraBodies,mvExtraBodyIds,iPhysicsBody*);
		mvExtraBodies.clear();
		cContainerListIterator<int> it = pData->mvExtraBodyIds.GetIterator();
		while(it.HasNext())
		{
			int lId = it.Next();
			iPhysicsBody* pBody = static_cast<iPhysicsBody*>(apSaveObjectHandler->Get(lId));
			mvExtraBodies.push_back(pBody);
			if(pBody==NULL)
			{
				Warning("Couldn't find save object with id %d\n",lId);
			}
		}

		//Make sure all bodies are setup
		SetCollideCharacter(mbCollideCharacter);
	}

	//-----------------------------------------------------------------------

}
